#ifndef __HYBRID_H_
#define __HYBRID_H_

#include "../liolib/lioflat.h"
#include "../liolib/lionav.h"
#include "../liolib/liocom.h"

//#define DEBUG
#define N_TRIANG 4
#define C_VERSION '1.02'
#define C_HEADER 'capt'
#define DEF_CUBE_SIZE 200
#define DEF_COR_PERIOD 100
#define DEF_SAMPLE_TIME 1e-3
#define FAST_PERIOD 100
#define DEF_COUNT 10000
#define DEF_RECURS TRUE
#define DEF_DEV_R 0.001
#define DEF_DEV_V 0.0001
#define DEF_DEV_Q 0.00001
#define DEF_BETA_A 100
#define DEF_DEV_A 1e-2
#define DEF_BETA_W 0.001
#define DEF_DEV_W 1e-3
#define DEF_BETA_M 1000
#define DEF_DEV_M 20
#define DEF_TRAJECT_FILE_NAME _T("traject.trj")
#define COLOR1 INT_DGREEN
#define COLOR2 INT_RED
#define COLOR3 INT_BLUE
#define COLOR4 INT_MAGENTA

namespace Lio {

class Hybrid:public FlatApplication
{
	enum captureIndice {
		C_TIME,
		C_X,C_Y,C_Z,
		C_LAT,C_LON,C_ALT,
		C_VNX,C_VNY,C_VNZ,
		C_QBN_0,C_QBN_1,C_QBN_2,C_QBN_3,
		C_QBI_0,C_QBI_1,C_QBI_2,C_QBI_3,
		C_ACCEL_X,C_ACCEL_Y,C_ACCEL_Z,
		C_GYRO_ROLL,C_GYRO_PITCH,C_GYRO_YAW,
		C_INERT_A_X,C_INERT_A_Y,C_INERT_A_Z,
		C_WEN_N_X,C_WEN_N_Y,C_WEN_N_Z,
		C_SIZE};
	enum playIndice {
		P_TIME,

		P_ERR_MES_ECEF,P_ERR_EST_ECEF,P_ERR_COR_ECEF,
		P_ERR_EST_VN,P_ERR_COR_VN,
		P_ERR_EST_EULER,P_ERR_COR_EULER,

		P_REAL_ACCEL_X,P_REAL_ACCEL_Y,P_REAL_ACCEL_Z,
		P_INPUT_ACCEL_X,P_INPUT_ACCEL_Y,P_INPUT_ACCEL_Z,
		P_REAL_GYRO_ROLL,P_REAL_GYRO_PITCH,P_REAL_GYRO_YAW,
		P_INPUT_GYRO_ROLL,P_INPUT_GYRO_PITCH,P_INPUT_GYRO_YAW,

		P_REAL_LAT,P_REAL_LON,P_REAL_ALT,
		P_EST_LAT,P_EST_LON,P_EST_ALT,
		P_MES_LAT,P_MES_LON,P_MES_ALT,
		P_COR_LAT,P_COR_LON,P_COR_ALT,
		
		P_REAL_X,P_REAL_Y,P_REAL_Z,
		P_EST_X,P_EST_Y,P_EST_Z,
		P_MES_X,P_MES_Y,P_MES_Z,
		P_COR_X,P_COR_Y,P_COR_Z,
		P_ERR_EST_X,P_ERR_EST_Y,P_ERR_EST_Z,
		P_ERR_MES_X,P_ERR_MES_Y,P_ERR_MES_Z,
		P_ERR_COR_X,P_ERR_COR_Y,P_ERR_COR_Z,
		
		P_REAL_VNX,P_REAL_VNY,P_REAL_VNZ,
		P_EST_VNX,P_EST_VNY,P_EST_VNZ,
		P_COR_VNX,P_COR_VNY,P_COR_VNZ,
		P_ERR_EST_VNX,P_ERR_EST_VNY,P_ERR_EST_VNZ,
		P_ERR_COR_VNX,P_ERR_COR_VNY,P_ERR_COR_VNZ,
		
		P_REAL_ROLL,P_REAL_PITCH,P_REAL_YAW,
		P_EST_ROLL,P_EST_PITCH,P_EST_YAW,
		P_COR_ROLL,P_COR_PITCH,P_COR_YAW,
		P_ERR_EST_ROLL,P_ERR_EST_PITCH,P_ERR_EST_YAW,
		P_ERR_COR_ROLL,P_ERR_COR_PITCH,P_ERR_COR_YAW,
		
		P_dLAT,P_dLON,P_dALT,
		P_dVNX,P_dVNY,P_dVNZ,
		P_dROLL,P_dPITCH,P_dYAW,

		P_NOISE_ACCEL_X,P_NOISE_ACCEL_Y,P_NOISE_ACCEL_Z,
		P_NOISE_GYRO_ROLL,P_NOISE_GYRO_PITCH,P_NOISE_GYRO_YAW,
		P_NOISE_MES_LAT,P_NOISE_MES_LON,P_NOISE_MES_ALT,
		P_NOISE_INPUT_TRACE,
		
		P_SIZE};

	FlatComponent 
		panel;
	FlatLabel
		lCorrect,
		lDevR,
		lDevV,
		lDevQ,
		lDevA,
		lDevW,
		lBetaA,
		lBetaW,
		lDevM,
		lBetaM;
	FlatText 
		tbCount,
		tbCorrect,
		tbSample,
		tbDevR,
		tbDevV,
		tbDevQ,
		tbDevA,
		tbBetaA,
		tbDevW,
		tbBetaW,
		tbDevM,
		tbBetaM;
	FlatButton 
		bNoise,
		bDef,
		bRecurs,
		bCount,
		bPeriod,
		bSample,
		bCapture,
		bPlay,
		bStop,
		bStep,
		bLoop,
		bGraphCapt,
		bGraphPlay,
		bReset,
		bAdjust,
		bAuto,
		bSaveBin,
		bExportGraph,
		bLoad,
		bFull,
		bScreen,
		bExit,
		bFast;
	FlatGraph 
		fgCapture,
		fgPlay;
	Limit<double> limX,limY,limZ;
	Channel *chnlCapt[C_SIZE],*chnlPlay[P_SIZE];
	int		kaccel,seed,
			counter,captureCount,captureIndex,
			correctPeriod;
	double	dt,
			captureData[C_SIZE],playData[P_SIZE],
			cubeSize,spanReduct,
			input_Wnb_b[3],qi[4],
			qi_play[4],re_play[3],
			ecef0[3],r0_play[3],v0_play[3],q0_play[4],
			input_measure[3],v_measure[3],
			dr[3],dv[3],de[3],
			input_accel[3],input_gyro[3],
			noise_r[3],noise_v[3],noise_q[3],
			noise_accel[3],noise_gyro[3],
			dev_r,noise_dev[15],
			dev_v,dev_q,
			beta_a,dev_a,
			beta_w,dev_w,
			beta_m,dev_m_lat,dev_m_lon,dev_m_alt,
			step_w;
	static CSTR captureNames[C_SIZE],playNames[P_SIZE];
	static int	objSimColors[N_TRIANG],objPlayColors[N_TRIANG],
				captureColors[C_SIZE],playColors[P_SIZE];
	static float vertices0[N_TRIANG][3][3],LightAmbient0[4],LightDiffuse0[4],LightPosition0[4];

	GLfloat LightAmbient[4],LightDiffuse[4],LightPosition[4];
	BOOL
		light,pause,pp,
		tabp,lp,retp,
		loadWait,saveWait;
	Matrix *mShow,noise_measure ;
	String mName,trajectFileName;
	GLfloat	step_rot,glRot[3],glTrans0[3],glTrans1[3];
	Font fontSmall,fontMatrix;
	GaussMarkov gm_a[3],gm_w[3],gm_m[3];
	float vertices[N_TRIANG][3][3],normal[3];
	INS ins;

	void reshape();
	void reshapeGraphs();
	void init();
	void initSim();
	void initPlay();
	void initCapture();
	void updateIns(double sample, const double *input_accel, const double *input_gyro);
	void setCaptureData();
	void breaks();
	void drawTraject();
	void drawObject(double * qi, double * ecef, int * colors);
	BOOL capture();
	BOOL play();
	void clearTraject();
	BOOL save(FILE *file);     
	BOOL load(FILE *file);
	BOOL create();
	void draw2D();
	void draw3D();
	void destroy();
	BOOL glCreate();
	void glDestroy();
	void update();
	BOOL listen(WORD id, WORD code, const void *info);
	BOOL setCount(int newCount);
	BOOL setNoise(	double newDevR, double newDevV, double newDevQ,
					double newDevA, double newBetaA, double newDevW, double newBetaW,
					double newDevM, double newBetaM);
	void setSampleTime(double newSampleTime);
	void updateNoise();
	void setPeriods(int newCorrectPeriod);
	void setCapture(int capture, const double *values, int index, int count = 1)
	{
		while((--count)>=0)
			chnlCapt[index+count]->elems[capture]=values[count];
	}
	void getCapture(int capture ,double *values, int index, int count = 1)
	{
		while((--count)>=0)
			values[count]=chnlCapt[index+count]->elems[capture];
	}
	void adjustTraject()
	{
		double tmp=maxval(maxval(limX.span(),limY.span()),limZ.span());
		if(tmp<=0)
			return;
		cubeSize=maxval(maxval(limX.span(),limY.span()),limZ.span());
		spanReduct=2.0/cubeSize;
	}
};

};

#endif